#include "crc16.h"

uint16_t modbus_crc16(uint8_t *_pBuf, uint16_t _usLen)
{
    uint16_t crc16 = 0xffff;
    for (int i = 0; i < _usLen; i++) {
        crc16 ^= ((uint16_t)_pBuf[i & 0x00ff]);
        for (int j = 0; j < 8; j++) {
            crc16 >>= 1;
            if ((crc16 & 0x0001) != 0) {
                crc16 ^= 0xa001;
            }
        }
    }

    return crc16;
}